#include "rclcpp/rclcpp.hpp"
#include <fstream>
#include <vector>
#include "geometry_msgs/msg/pose_stamped.hpp"
// 自定义
#include "nav_msg_pkg/msg/waypoint.hpp"
#include "rviz_util_pkg/rviz_marker.hpp"
#include "agv_util/point_util.hpp"
class EditPoint : public rclcpp::Node
{
public:
    EditPoint() : Node("edit_point_node")
    {
        RCLCPP_INFO(logger_, "节点已经启动!");

        // 创建订阅者，订阅 add_point_tool 发布的点位消息
        point_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "/rviz_tools_pkg/add_point",
            10,
            std::bind(&EditPoint::pointCallback, this, std::placeholders::_1));
        rviz_marker_ptr_ = std::make_shared<rviz_util_pkg::RvizMarker>(this);
        // 延迟加载点
        rclcpp::sleep_for(std::chrono::seconds(5));
        points_ = agv_util::load_waypoint();
        rclcpp::WallRate rate(5); // hz
        for (auto &point : points_)
        {
            publish_marker(point);
            rate.sleep();
        }
    }

private:
    void pointCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {
        nav_msg_pkg::msg::Waypoint point;
        point.frame_id = msg->header.frame_id;
        point.pose = msg->pose;
        point.name = "point_" + std::to_string(points_.size() + 1);
        publish_marker(point);
        // 存储点数据
        points_.push_back(point);
        agv_util::save_waypoint(points_);
    }

    void publish_marker(const nav_msg_pkg::msg::Waypoint &point)
    {
        double x = point.pose.position.x;
        double y = point.pose.position.y;
        // 发布
        rviz_marker_ptr_->publish_marker_point(x, y, point.name);
        rviz_marker_ptr_->publish_marker_txt(x, y, point.name);
    }

    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr point_sub_;
    std::shared_ptr<rviz_util_pkg::RvizMarker> rviz_marker_ptr_;

    std::vector<nav_msg_pkg::msg::Waypoint> points_;
    rclcpp::Logger logger_ = this->get_logger();
};

int main(int argc, char *argv[])
{
    /* 初始化rclcpp  */
    rclcpp::init(argc, argv);
    /* 产生一个 节点 */
    auto node = std::make_shared<EditPoint>();
    /* 运行节点，并检测退出信号 Ctrl+C */
    rclcpp::spin(node);
    /* 停止运行 */
    rclcpp::shutdown();
    return 0;
}